#include "CommandThread.h"
#include "CommsDefs.h"


xQueueHandle CommandThreadQueue;
xTaskHandle CommandThreadHandle;
unsigned char State;
//
xTimerHandle xTimer;
void vTimerCallback( xTimerHandle pxTimer );
//


extern xQueueHandle UARTQueue;
extern xQueueHandle WebQueue;
extern xQueueHandle MappingThreadQueue;
unsigned char History[3];
Comms_Msg_Struct *Msg;
Comms_Msg_Struct MotorControlMsg;
Comms_Msg_Struct ToWebMsg;
Comms_Msg_Struct *MotoroutMsgPtr;
Comms_Msg_Struct *WeboutMsgPtr;
unsigned char SpeedOffset = 0;


void CommandThreadTaskCode( void * pvParameters )
{
        // History init, it holds the last three commands executed
        History[0] = 120;
        History[1] = 120;
        History[2] = 120;
        // Initialize 

        // initialize MotorController Message, later add the commands
        MotorControlMsg.msgType = MOTORCONTROLLER;
        MotorControlMsg.counter = 0;
        MotorControlMsg.size = 2;
        MotorControlMsg.data[0] = 0;
        MotorControlMsg.data[1] = 0;
        MotorControlMsg.data[2] = 0;
        MotorControlMsg.data[3] = 0;
        MotorControlMsg.data[4]  = 0;
        MotorControlMsg.data[5]  = 0;
        // initialize message that will be sent to the web upon calculating the command
        ToWebMsg.msgType = Command;
        ToWebMsg.counter = 0;
        ToWebMsg.size = 1;
        ToWebMsg.data[0] = 0;
        ToWebMsg.data[1] = 0;
        ToWebMsg.data[2] = 0;
        ToWebMsg.data[3] = 0;
        ToWebMsg.data[4]  = 0;
        ToWebMsg.data[5]  = 0;
        unsigned short distance;
        // subject to change upon testing
        
        unsigned char SpeedCMD = 0;

        MotoroutMsgPtr = &MotorControlMsg;
        WeboutMsgPtr = &ToWebMsg;
        State = None;

for(;;){

        if( CommandThreadQueue != 0 )
            {
                if( xQueueReceive( CommandThreadQueue,(void*) &( Msg ), ( portTickType ) 0 ) )
                   {
                        switch((Msg_Type_Enum)Msg->msgType){
                                        // Collision Thread Case
                                case Collision:
                                        CalculateCommand(Msg->data[0]);
                                        break;
                                        
                                case Speed:
                                        switch(Msg->data[0])
                                        {
                                         case Low:
                                                State = Low;
												//SpeedOffset = 2;
                                                break;
                                         case High:
                                                State = High;
												//SpeedOffset = 2;
                                                break;
                                         case Finish:
                                                State = Finish;
												//SpeedOffset = 0;
                                                break;
                                         case None:
										 		
                                                //State = None;
                                                break;
                                           //State = None;
                                         default:
										 		//SpeedOffset = 0;
                                                break;
                                        }
                                        break;
                                        
                                case Mapping:
                                        {
                                                AddMsgToHistory(Msg->data[0]);  
                                        }
                                        break;
                                        
                                default:        
                                        break;
                                        
                                }

                        }
                }
        }
}

void Init_CommandThread()
{
        xTaskHandle CommandThreadHandle;
        xTaskCreate( CommandThreadTaskCode, ( signed char * ) "Command ThreadTask", 2*configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, &CommandThreadHandle );
        CommandThreadQueue = xQueueCreate( 10, sizeof(Comms_Msg_Struct *)  );
}


void AddMsgToHistory(unsigned char msg){
        static int count = 0;
        unsigned char temp,temp2;
        temp = History[0];
        History[0] = msg;
        temp2 = History[1];
        History[1] = temp;
        History[2] = temp2;
}

void CalculateCommand(unsigned char msg){
        static unsigned char FinalCommand = 0;
        switch((Collision_Data_Enum)msg)
        {
                case TurnLeft:
                        FinalCommand = TurnLeft;
                        MotorControlMsg.data[0] = CMDTurnLeftL; 
                        MotorControlMsg.data[1] = CMDTurnLeftR;
                        break;

                case Straight:
                        FinalCommand = Straight;
                        switch(State)
                        {
                                 case Low:
                                        MotorControlMsg.data[0] = CMDStraightL - 2; 
                                        MotorControlMsg.data[1] = CMDStraightR - 2;
                                        break;
                                 case High:
                                        MotorControlMsg.data[0] = CMDStraightL + 2; 
                                        MotorControlMsg.data[1] = CMDStraightR + 2;
                                        break;
                                 case Finish:
                                        FinalCommand = Stop;
                                        MotorControlMsg.data[0] = CMDStopL; 
                                        MotorControlMsg.data[1] = CMDStopR;
                                        break;
                                        
                                 default:
                                        MotorControlMsg.data[0] = CMDStraightL; 
                                        MotorControlMsg.data[1] = CMDStraightR;
                                        break;
                        }
							if(FinalCommand == Straight){
                        		if(History[0] == Straight || History[1] == Straight || History[2] == Straight)
                                {
	                                MotorControlMsg.data[0] = CMDStraightL + 2; 
	                                MotorControlMsg.data[1] = CMDStraightR + 2;     
                                }
							}
                        break;
                case TurnRight:
                                FinalCommand = TurnRight;
                                MotorControlMsg.data[0] = CMDTurnRightL; 
                                MotorControlMsg.data[1] = CMDTurnRightR;
                        break;
                
                case Stop:
                        FinalCommand = Stop;
                        MotorControlMsg.data[0] = CMDStopL; 
                        MotorControlMsg.data[1] = CMDStopR;
                        
                        break;

                case Reverse:
                        FinalCommand = Reverse;
                        MotorControlMsg.data[0] = CMDReverseL; 
                        MotorControlMsg.data[1] = CMDReverseR;
                        
                        break;
                default:
                        // send stop command
                        FinalCommand = Stop;
                        MotorControlMsg.data[0] = CMDStopL; 
                        MotorControlMsg.data[1] = CMDStopR;
                        break;

        }
        // set calculated values in structs before sending them out
        // Setting message to web
        ToWebMsg.data[0] = FinalCommand;
        
        // Send command to the UART queue and the Web queue
        
        if( UARTQueue != 0 )
    {
        xQueueSend( UARTQueue, ( void * ) &MotoroutMsgPtr, ( portTickType ) 0 );
    }
        if( WebQueue != 0 )
    {
        xQueueSend( WebQueue, ( void * ) &WeboutMsgPtr, ( portTickType ) 0 );
    }
        if( MappingThreadQueue != 0 )
        {
                xQueueSend( MappingThreadQueue, ( void * ) &WeboutMsgPtr, ( portTickType ) 0 );
        }
        // Stop UART Tread
        if(State == Finish)
        {
                State = None;
                ToWebMsg.data[0] = 0;
                if( UARTQueue != 0 )
        {
        xQueueSend( UARTQueue, ( void * ) &WeboutMsgPtr, ( portTickType ) 0 );
        }
        }

}